/*
 * Team D closed-loop arm and intake control code.
 * @author Derek Leung
 */

//=====External=====//

/* Specification */

//arm constants
const int ARM_BOTTOM = 713;
const int ARM_MAXERR = 10;
const int ARM_30 = 2550;
const int ARM_20 = 1850;
const int ARM_11 = 1368;
const int ARM_10 = 1207;
const int ARM_6 = 987;
const int ARM_PMULT = 0.5;
const int ARM_DMULT = 0;
const int ARM_IMULT = 0;
const int ARM_IMAX = 40;
//distance from object for intake to run
const int ULTRA_MAX = 10;

//-----arm functions-----//

//target
int arm_target = ARM_BOTTOM;

//returns true when target is reached
bool arm_target_reached = true;

//=====Internal=====//
int real_pot;
int arm_movesign;
int arm_err=0;
int arm_errprev=0;
int arm_errtot=0;
int arm_power;
int arm_ppower;
int arm_ipower;
int arm_dpower;
int intake_direction=0;
bool ultra_range=false;
bool task_arm=false; //flag for regulate_arm
bool task_opauto=false: //flag for opauto

//sign-finding function
int sign(int num)
{
  if(num==0) return 0;
  else return abs(num)/num;
}

//ultrasonic range checker
void ultra_test()
{
  if(SensorValue(ultra) < ULTRA_MAX)
  {
    ultra_range = true;
  }
  else
  {
    ultra_range = false;
  }
}

//intake control
void regulate_intake(intake_direction)
{
  motor[port7] = 127*intake_direction;
}

//arm raising PID
task regulate_arm()
{
  task_arm = true;
  while(true)
  {
    real_pot = SensorValue(pot);
    arm_err = real_pot - arm_target;
    //arm_errprev = arm_err;
    //arm_errtot = arm_errtot + arm_err;
    //if(arm_errtot > ARM_IMAX)
    //  arm_errtot = 0;
    if(abs(arm_err) > ARM_MAXERR)
    {
      arm_target_reached = false;
      arm_ppower = -arm_err;
      //arm_ipower = arm_errtot;
      //arm_dpower = arm_err - arm_errprev;
      arm_power = ARM_PMULT*arm_ppower;
      //arm_power = ARM_PMULT*arm_ppower + ARM_IMULT*arm_ipower + ARM_DMULT*arm_dpower;
      if(arm_power > 127)
        arm_power = 127;
      else if(arm_power < -127)
        arm_power = -127;
      motor[port1] = -arm_power;
      motor[port3] = arm_power;
      motor[port8] = -arm_power;
      motor[port10]= arm_power;
      //wait1Msec(10);
    }
    else
    {
      arm_target_reached = true;
      break;
    }
  }
  task_arm = false;
}

//intaking operator automatic function
task opauto()
{
  task_opauto = true;
  arm_target = ARM_6;
  StartTask (regulate_arm);
  ultra_test();
  if(ultra_range)
  {
    regulate_intake(1);
    arm_target = ARM_BOTTOM;
    StartTask (regulate_arm);
  }
  task_opauto = false
}

//checking for button presses
task arm_control()
{
  while(true)
  {
    if(vexRT[Btn7UXmtr2] == 1)
    {
     	arm_target = ARM_30;
     	if(!task_arm) StartTask (regulate_arm);
    }
    else if(vexRT[Btn7LXmtr2] == 1)
    {
    	arm_target = ARM_20;
     	if(!task_arm) StartTask (regulate_arm);
    }
    else if(vexRT[Btn7RXmtr2] == 1)
    {
	    arm_target = ARM_11;
     	if(!task_arm) StartTask (regulate_arm);
    }
    else if(vexRT[Btn7DXmtr2] == 1)
    {
     	if(!task_opauto) StartTask (opauto);
    }
    else
    {
      StopTask(regulate_arm);
      StopTask(opauto);
      task_arm = false;
      task_opauto = false;
      arm_target_reached = true;
    }
  }
}
